05. Waypoint Updater Node (Partial)
Waypoint Updater Node Overview
Before the car can move in the simulator, it will be necessary to write a first version of the
waypoint_updater
node. You can find class and method stubs for the node in
(path_to_project_repo)/ros/src/waypoint_updater/waypoint_updater.py
The eventual purpose of this node is to publish a fixed number of waypoints ahead of the vehicle with the correct target velocities, depending on traffic lights and obstacles. The goal for the first version of the node should be simply to subscribe to the topics
-
/base_waypoints
-
/current_pose
and publish a list of waypoints to
-
/final_waypoints
The
/base_waypoints
topic publishes a list of all waypoints for the track, so this list includes waypoints both before and after the vehicle (note that the publisher for
/base_waypoints
publishes only once). For this step in the project, the list published to
/final_waypoints
should include just a fixed number of waypoints currently ahead of the vehicle:
-
The first waypoint in the list published to
/final_waypoints
should be the first waypoint that is currently ahead of the car. -
The total number of waypoints ahead of the vehicle that should be included in the
/final_waypoints
list is provided by theLOOKAHEAD_WPS
variable inwaypoint_updater.py
.
The next section includes details about the message type used to publish to
/final_waypoints
.
Waypoint Message Descriptions
This section will demonstrate use of
rostopic
and
rosmsg
to learn more about the messages being transmitted on
/base_waypoints
and
/final_waypoints
. If you are already familiar with these ROS commands and feel comfortable exploring ROS messages on your own, feel free to skip this section!
From the code in
waypoint_updater.py
, we can see that both the
/final_waypoints
and
/base_waypoints
topics have message type
Lane
. You can look at the details about this message type in
<path_to_project_repo>/ros/src/styx_msgs/msg/
, but this can also be done from the command line after launching the ROS project using
rostopic
and
rosmsg
as follows:
After opening a new terminal window and sourcing
devel/setup.bash
, you can see see all topics by executing:
$ rostopic list
You should see
/final_waypoints
among the topics. Executing:
$ rostopic info /final_waypoints
will provide info about the message type being used in
/final_waypoints
:
Type: styx_msgs/Lane
Next executing:
$ rosmsg info styx_msgs/Lane
provides the following message information:
std_msgs/Header header
uint32 seq
time stamp
string frame_id
styx_msgs/Waypoint[] waypoints
geometry_msgs/PoseStamped pose
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
geometry_msgs/TwistStamped twist
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
From here you can see that the messages contain a
header
and a
Waypoint
list named
waypoints
. Each waypoint has
pose
and
twist
data. Going further, you can see that
twist.twist
data contains 3D
linear
and
angular
velocities. For more information about twist messages,
see documentation here
.
Check to be sure you can explore the messages sent in
/base_waypoints
and
/current_pose
as well using the steps above!
Lane message example
As a use-case example, given a single
styx_msgs/Lane
message
my_lane_msg
, you can access the x direction linear velocity of the first waypoint in Python with:
my_lane_msg[0].twist.twist.linear.x
Note that the coordinates for linear velocity are vehicle-centered, so only the x-direction linear velocity should be nonzero.
Topics and message types
For convenience, we have provided the following table with topic and message info for this step of the project:
Topic | Msg Type | Notes |
---|---|---|
/base_waypoints | styx_msgs/Lane | Waypoints as provided by a static .csv file. |
/current_pose | geometry_msgs/PoseStamped | Current position of the vehicle, provided by the simulator or localization. |
/final_waypoints | styx_msgs/Lane | This is a subset of /base_waypoints. The first waypoint is the one in /base_waypoints which is closest to the car. |